#include <iostream>
#include <iomanip>
#include <stdlib.h>
#include <time.h>
#include <stdio.h>
#include <semaphore.h>
#include <arm_neon.h>

#include <random>
#include <chrono>
#include <omp.h>

using namespace std;

const int n = 512;
const int m = 512;
const int p = 64;
const int NUM_THREADS = 6;

int global_i;
int R[n][n];
int E[n][n];
int FirstNum;

int lp(int *A) {
    int x = n - 1;
    int count = 0;
    while (!A[count]) {
        x--;
        count++;
    }
    return x;
}

void threadFunc(int t_id) {
    while (global_i < m) {
        #pragma omp critical
        {
            int avgnum = n / NUM_THREADS;
            int begin = t_id * avgnum;
            int end = (t_id + 1) * avgnum;
            if (t_id == NUM_THREADS - 1)
                end = n;
            int32x4_t vtE;
            int32x4_t vtR;
            int j;
            for (j = begin; j + 4 < end; j += 4) {
                vtE = vld1q_s32(&E[global_i][j]);
                vtR = vld1q_s32(&R[FirstNum][j]);
                vtE = veorq_s32(vtE, vtR);
                vst1q_s32(&E[global_i][j], vtE);
            }
            for (; j < end; j++)
                E[global_i][j] = E[global_i][j] ^ R[FirstNum][j];
        }
        #pragma omp barrier
        #pragma omp single
        global_i++;
    }
}

void initialize_matrices() {
    srand((unsigned int)time(NULL));
    for (int i = 0; i < n; i++)
        for (int j = 0; j < n; j++) {
            R[i][j] = 0;
            E[i][j] = 0;
        }
    for (int i = 0; i < m; i++)
        for (int j = 0; j < n; j++)
            E[i][j] = rand() & 1;

    for (int i = 0; i < p; i++) {
        int NowRow = rand() % n;
        R[NowRow][n - NowRow - 1] = 1;
        for (int j = n - NowRow; j < n; j++)
            R[NowRow][j] = rand() & 1;
    }
}

int main() {
    initialize_matrices();

    auto start = chrono::high_resolution_clock::now();

    #pragma omp parallel num_threads(NUM_THREADS)
    {
        int t_id = omp_get_thread_num();
        threadFunc(t_id);
    }

    auto end = chrono::high_resolution_clock::now();
    auto diff = chrono::duration_cast<chrono::duration<double, milli>>(end - start);
    cout << "Size = " << n << ": " << diff.count() << "ms" << endl;
    cout << endl;

    return 0;
}
